#include "PathFollowBehaviour.h"
#include "BehaviourInfo.h"
#include "BPhysicsActor.h"
#include "ArriveBehaviour.h"
#include "SeekBehaviour.h"
#include "Grid.h"

void PathFollowBehaviour::CalcMotion(float* motion, BehaviourInfo* info, float weighting)
{
    if(!info->GetSourceActor() || info->GetTargetPathNode() == -1 ||
        info->GetGrid() == NULL)
        return;

    int targetNode = info->GetTargetPathNode();
    btVector3 sPos = info->GetSourceActor()->GetPosition();
    std::list<int>& path = info->GetPath();
    D3DXVECTOR3 startPos(sPos.x(), sPos.y(), sPos.z());

    if(targetNode == -1 || targetNode != info->GetCurrentTarget())//we do not yet have a path
    {
        path.clear();
        int start = info->GetGrid()->GetNearestNode(startPos, info->GetSourceActor()->GetAiCollisionRadius());
        bool res = info->GetGrid()->AStarSearch(start, info->GetTargetPathNode(), &path);
        if(!res)
            return;
        std::list<int>::iterator it = path.begin();
        info ->SetCurrentTarget(*it);
        path.reverse();
    }
    if(path.size() > 1)
    {
        std::list<int>::iterator it = path.begin();
        int start = (*it);
        ++it;
        int next = (*it);
        while(it != path.end())
        {   
            info->GetGrid()->DrawEdge(start, next, D3DXVECTOR3(0,0,1));
            info->GetGrid()->DrawNode(start, D3DXVECTOR3(0,1,0));
            info->GetGrid()->DrawNode(next, D3DXVECTOR3(0,1,0));
            start = next;
            ++it;
            if(it != path.end())
                next = (*it);
        }
    }
    //we have a path, now check if we need to update our node
    int nextNode = (*path.begin());
    D3DXVECTOR3 toNode = startPos - info->GetGrid()->GetGraph().Nodes()[nextNode].GetPos();
    if(toNode.x == 0 && toNode.y == 0 && toNode.z == 0)
        toNode.x = 1;
    float dist = D3DXVec3LengthSq(&toNode);
    if(dist < 400.0f )
    {
        //we are close to this node, so we want to head to the next one
        if(path.size() > 1)
        {
            std::list<int>::iterator it = path.begin();
            it = path.erase(it);
        }
        if(path.size() == 0)
            info->SetTargetPathNode(-1); //we are here!
    }
    if(path.size() == 1)
    {
        //if seek is a valid object, but i is a seek type poiner
        if(m_seek && m_seekType == 0)//0 for seek
        {
            delete m_seek;
            m_seek = new ArriveBehaviour();
            m_seekType = 1;
        }
        else if(!m_seek)//first time here then
        {
            m_seek = new ArriveBehaviour();
            m_seekType = 1;
        }
    }
    else
    {
        if(m_seek && m_seekType == 1)//1 for arrive
        {
            delete m_seek;
            m_seek = new SeekBehaviour();
            m_seekType = 0;
        }
        else if(!m_seek)//first time here then
        {
            m_seek = new SeekBehaviour();
            m_seekType = 0;
        }
    }

    info->SetTargetActor(NULL);
    D3DXVECTOR3 targetPos = info->GetGrid()->GetGraph().Nodes()[(*path.begin())].GetPos();
    btVector3 targetPosBT(targetPos[0],targetPos[1],targetPos[2]);
    info->SetTargetPosition(&targetPosBT);
    m_seek->CalcMotion(motion, info, weighting);
    delete m_seek;
    m_seek = NULL;
}